clear;clc;
bag_icon=rosbag('C:\Users\10071\Desktop\rosbag\2021-04-07-19-40-02.bag');
bag_UWB=rosbag('C:\Users\10071\Desktop\rosbag\2021-04-07-19-40-09.bag');

state_select_icon= select(bag_icon, 'Time',[bag_icon.StartTime bag_icon.EndTime], 'Topic', '/mocap/pose_01');
stateMsgs_icon= readMessages(state_select_icon);

state_select_UWB= select(bag_UWB, 'Time',[bag_UWB.StartTime bag_UWB.EndTime], 'Topic', '/uwb_tag_pos');
stateMsgs_UWB= readMessages(state_select_UWB);
% dis_select_UWB= select(bag_UWB, 'Time',[bag_UWB.StartTime bag_UWB.EndTime], 'Topic', '/nlink_linktrack_nodeframe3');
% disMsgs_UWB= readMessages(dis_select_UWB);

ts_state_icon= timeseries(state_select_icon, 'Pose.Position.X','Pose.Position.Y','Pose.Position.Z');
ts_state_UWB= timeseries(state_select_UWB, 'Pose.Position.X','Pose.Position.Y','Pose.Position.Z');

pose_i_z = ts_state_icon.Data(204:4754,3);  % 第204个数据对应的时间为1617795609.448063
pose_U_z = ts_state_UWB.Data(:,3);          % 第一个数据对应的时间为1617795609.438247
pose_i_x = ts_state_icon.Data(204:4754,1);
pose_U_x = ts_state_UWB.Data(:,1);
pose_i_y = ts_state_icon.Data(204:4754,2);
pose_U_y = ts_state_UWB.Data(:,2);

% plot state in relative time.
t_relative_UWB= ts_state_UWB.Time - ts_state_UWB.Time(1,1); 
t_relative_icon= ts_state_icon.Time(204:4754,1) - ts_state_icon.Time(204,1); 
subplot(2,2,3);
hold on;
plot(t_relative_UWB, pose_U_z);
plot(t_relative_icon, pose_i_z);
ylabel('z');
subplot(2,2,1);
hold on;
plot(t_relative_UWB, pose_U_x);
plot(t_relative_icon, pose_i_x);
ylabel('x');
subplot(2,2,2);
hold on;
plot(t_relative_UWB, pose_U_y);
plot(t_relative_icon, pose_i_y);
ylabel('y');


